/*
 * File:         CR1.cpp
 * Date:         2011 03 29
 * Description:  Controller for Robot type 1
 * Author:       Gonzalo R. Canosa
 *
 *
 */

#include "CR1.h"

ConR::ConR() {
  printf("Comienza inicializacion\n");
  time_step = getBasicTimeStep();
  webots::Robot* R = this;
  webots::DifferentialWheels* DF = this;
  RWL.init(R,&n,"lidar");
  RWRangeCam.init(R,&n,"range_camera");
  RWKinectCam.init(R,&n,"kinect");
  RWCam.init(R,&n,"camera");
  RWPio.init(DF,&n,"ds");
  //RWJoint.init(R,&n,"pioneer");

}

void ConR::run()
{
  //ACTIVE ROS PUBLICSHER
  printf("Run\n");
 // std::stringstream ss;
 // std_msgs::String msg;

  //s.str(std::string()); PARA VACIAR

  ros::Rate loop_rate(10);
  printf("Funciona?\n");
  ROS_INFO("Funciona?");
  while(true)
  {
    step(time_step);
    RWL.iterate();
    RWRangeCam.iterate();
   // RWKinectCam.iterate();
    RWCam.iterate();
    RWPio.iterate();
  // RWJoint.iterate();
    loop_rate.sleep();
  }

//  //GET RANGE MATRIX
//  double range_matrix[range_width][range_height];
//  range_image = RANGE->getRangeImage();
//  for(int i=0;i<range_width;i++)
//  {
//    for(int j = 0;j<range_height;j++){
//      distance = RANGE->rangeImageGetDepth(range_image,range_width,i,j);
//      if (distance > 79.9)
//        distance = 80;
//      range_matrix[i][j]=distance;
//    }



}


int main(int argc, char **argv)
{
  //Lanzando otro hilo antes de éste no es necesario lanzar roscore desde
  //la consola.
  int roslaunch = fork();
  if(roslaunch == 0)
  {
    execlp("roslaunch","roslaunch","datmo","datmo.launch",NULL);
    return 0;
  }

//  int roslaunch2 = fork();
//  if(roslaunch2 == 0)
//  {
//    execlp("roslaunch","roslaunch","robotstate.launch",NULL);
//    return 0;
//  }

  ros::init(argc, argv, "CR1Webots");

  ConR *controller = new ConR();

  // launch the joy ROS node
    //  int roslaunch=fork();
    //  if (roslaunch==0) { // child process
    //   execlp("roslaunch","roslaunch","joy.launch",NULL);
    //   return 0;
    // }
  /*
   controller::step(1);
  ros::spinOnce();
	chatter_pub.publish(msg);
   controller::step(1);
  ros::spinOnce();
	chatter_pub.publish(msg);
   */
  controller->run();
  delete controller;
  ros::shutdown();
  //kill(roslaunch,SIGINT);
  return 0;
}




